// Tests using serial port + XBee to communicate between cubes

#include "send.h"

xMsg * createMessage(uint8_t to, uint8_t from, uint8_t size, uint8_t * data)
{
	xMsg * tempMsg = new xMsg; 
	tempMsg->callCard= CALL_CARD; 
	tempMsg->toAddr = to; 
	tempMsg->fromAddr = from; 
	tempMsg->size = size; 
	tempMsg->confirmation = 0x00;				// 0 is not a confirmation. 0xAA is confirmation
	
	// Contains header information for hashing
	uint8_t * tempHeaderBuff = new uint8_t[HEADER_SIZE];	// There are 5 bytes of header 	
	memcpy(tempHeaderBuff, &tempMsg->callCard, 1);
	memcpy(tempHeaderBuff+1, &to, 1);
	memcpy(tempHeaderBuff+2, &from, 1);
	memcpy(tempHeaderBuff+3, &size, 1);
	memcpy(tempHeaderBuff+4, &tempMsg->confirmation, 1);
	MD4(tempHeaderBuff, HEADER_SIZE, tempMsg->headerDigest);// Hashes header and stores in headerDigest
	delete(tempHeaderBuff); 

	MD4(data, MAX_MSG_SIZE, tempMsg->messageBodyDigest);	// Creates digest and puts it into struct

	memset(tempMsg->messageBody, 0x00, MAX_MSG_SIZE);	// Sets all memory body to 0
	memcpy(tempMsg->messageBody, data, size);		// Copies data into message body
	MD4(data, MAX_MSG_SIZE, tempMsg->messageBodyDigest);	// Creates digest and puts it into struct
	
	return tempMsg; 
}


int sendMessage(xMsg * msg, int fd)
{
    // Sends message

	for(int i=0; i<sizeof(xMsg); i++)
	{
		serialPutchar(fd, ((uint8_t *)((void *)msg))[i]); 
	}

	// wait for reply
	// if successful
		// GOOD!
		return 0;
	// else
		//retry 
}




int main(int argc, char ** argv)
{
	uint8_t dataBuffer[MAX_MSG_SIZE];
	// Make message
	for(int i=0; i<MAX_MSG_SIZE; i++)
	{
		dataBuffer[i] = (uint8_t)i; 
	}
 
	xMsg * currentMsg = createMessage(1, 0, MAX_MSG_SIZE, dataBuffer);

	int fd = serialOpen(SER_PORT, BAUD_RATE); 
	if(fd >= 0){
		for(int i=0; i<sizeof(xMsg); i++)
		{
			serialPutchar(fd, ((uint8_t *)((void *)currentMsg))[i]); 
		} 
	}else{
		std::cout << "Error Opening USB" << std::endl; 	
	}
	
	
	return 0; 
}
